#!/usr/bin/env python
from __future__ import print_function

import os
import sys
from collections import namedtuple

import rospy
from gazebo_msgs.msg import ModelStates
from geometry_msgs.msg import Twist

from controller.builder import create_controller
from trajectory.builder import create_trajectory

SimInfo = namedtuple('SimInfo', 'time max_v, max_w')

DELTA_T = 0.15  # this is the sampling time
SIM_INFO = {
    'linear': SimInfo(60.0, 0.076, 1.35),
    'circular': SimInfo(300.0, 0.11, 1.25),
    'squared': SimInfo(300.0, 0.11, 1.25),
    'lemniscate': SimInfo(300.0, 0.125, 1.25),
    'epitrochoid': SimInfo(600.0, 0.162, 1.25),
    'lissajous': SimInfo(300.0, 0.162, 1.25),
    'master_trajectory_save': SimInfo(800.0, 4.5, 1.25)
    'master_trajectory_point': SimInfo(800.0, 4.5, 1.25)
}

IMPLEMENTED = {
    'euler':
        ('linear', 'circular', 'squared',
         'lemniscate', 'epitrochoid', 'lissajous','master_trajectory_save','master_trajectory_point'),
    'pid':
        ('linear','master_trajectory_save'),
}


def get_pose(message):
    global current_pose, current_twist
    current_pose = message.pose[-1]
    current_twist = message.twist[-1]


def compute_control_actions():
    global i
    controller.compute_control_actions(current_pose, current_twist, i)
    twist = Twist()
    twist.linear.x = controller.v_c_n
    twist.angular.z = controller.w_c_n
    if twist.linear.x > 4.5:
        twist.linear.x = 4.5
    if twist.linear.x <  -4.5:
        twist.linear.x = -4.5
    if twist.angular.z > 2:
        twist.angular.z = 2
    if twist.angular.z < -2:
        twist.angular.z = -2
    # if current_pose.position.x > 90 and current_pose.position.x < 120 and current_pose.position.y > -10 and current_pose.position.y < 10:
    #     twist.linear.x = 0.5
    #     twist.angular.z = 0.


    twist_publisher.publish(twist)
    detal_index =  2
    i += detal_index


if __name__ == '__main__':

    CONTROLLER = "euler"
    TRAJECTORY = "master_trajectory_save"
    TRAJECTORY = "master_trajectory_point"
    PERIOD = SIM_INFO[TRAJECTORY].time    #one circle time   change to simulation time
    MAX_V = SIM_INFO[TRAJECTORY].max_v
    MAX_W = SIM_INFO[TRAJECTORY].max_w

    # try:
    #     SIM_TIME = float(800)

    #     if SIM_TIME <= 0:
    #         print('Error: simulation time must be a positive number!')
    #         sys.exit(-4)

    # except ValueError:
    #     print('Error: simulation time must be a valid number!')
    #     sys.exit(-5)
    # except IndexError:
    #     SIM_TIME = PERIOD

    SIM_TIME = PERIOD
    STEPS = int(SIM_TIME / DELTA_T)

    rospy.init_node('control')
    current_pose = None
    current_twist = None
    subscriber = rospy.Subscriber('gazebo/model_states', ModelStates, get_pose)
    twist_publisher = rospy.Publisher('computed_control_actions',
                                      Twist,
                                      queue_size=1)

    while current_pose is None or current_twist is None:
        pass

    i = 0
    trajectory = create_trajectory(TRAJECTORY, PERIOD, DELTA_T)
    controller = create_controller(trajectory, CONTROLLER, DELTA_T, SIM_INFO)
    rate = rospy.Rate(int(1 / DELTA_T))
    while not rospy.is_shutdown() and i < STEPS:
        compute_control_actions()
        rate.sleep()

    rospy.spin()
